using System;
using System.Collections.Generic;
using System.Linq;

namespace LegoBotPrimaAbstractions.Extensions
{
    public static class SensorData_Ext
    {
        public const double SPEED_TIME_MS = 1000;

        public static  double CalculateAngleChangeSpeed(this IEnumerable<SensorData> data)
        {
            return data.CalculateSpeed((curr, prev) => curr.Angle - prev.Angle);
        }

        public static double CalculateDistanceChangeSpeed(this IEnumerable<SensorData> data)
        {
            return data.CalculateSpeed((curr, prev) => curr.Distance - prev.Distance);
        }

        private static double CalculateSpeed(this IEnumerable<SensorData> data, Func<SensorData, SensorData, double> getValueDiff)
        {
            SensorData prevData = null;
            var speeds = new LinkedList<double>();
            foreach (var currentData in data)
            {
                if (prevData == null)
                {
                    prevData = currentData;
                    continue;
                }
                var speed = currentData.CalculateSpeed(prevData, getValueDiff);
                speeds.AddLast(speed);
                prevData = currentData;
            }
            if (speeds.Count == 0)
            {
                throw new ArgumentException("In order to calculate speed you need more than one entry", "data");
            }

            return speeds.Sum() / speeds.Count;
        }


        public static void DefineAngleByCounterclockwiseNeighbours(this SensorData distance, SensorData prevAngle, SensorData nextAngle)
        {
            distance.DefineAngleByNeighbours(prevAngle, nextAngle,
                                            d =>
                                            {
                                                d = d % 360;
                                                if (d > 0)
                                                    d -= 360;
                                                return d;
                                            });
        }

        public static void DefineAngleByClockwiseNeighbours(this SensorData distance, SensorData prevAngle, SensorData nextAngle)
        {
            distance.DefineAngleByNeighbours(prevAngle, nextAngle,
                                            d =>
                                            {
                                                d = d % 360;
                                                if (d < 0)
                                                    d += 360;
                                                return d;
                                            });
        }

        private  static void DefineAngleByNeighbours(this SensorData distance, SensorData prevAngle, SensorData nextAngle, Func<double, double> correct)
        {
            if (prevAngle.TimeStamp > distance.TimeStamp || nextAngle.TimeStamp < distance.TimeStamp)
            {
                throw new ArgumentException("Supplied sensor data is not neibouring");
            }

            var interval = nextAngle.TimeStamp - prevAngle.TimeStamp;
            var position = distance.TimeStamp - prevAngle.TimeStamp;

            if (interval.TotalMilliseconds == 0)
            {
                distance.Angle = nextAngle.Angle;
                return;
            }

            var ratio = position.TotalMilliseconds / interval.TotalMilliseconds;

            var diff = MathUtil.AnglesDiff(prevAngle.Angle, nextAngle.Angle);
            diff = correct(diff);
            var angle = (prevAngle.Angle + diff * ratio) % 360;
            distance.Angle = (angle < 0) ? angle + 360 : angle;
        }

        public static double CalculateAngleChangeSpeed(this SensorData current, SensorData previous)
        {
            return current.CalculateSpeed(previous, (curr, prev) => curr.Angle - prev.Angle);
        }

        public static double CalculateDistanceChangeSpeed(this SensorData current, SensorData previous)
        {
            return current.CalculateSpeed(previous, (curr, prev) => curr.Distance - prev.Distance);
        }

        private static double CalculateSpeed(this SensorData current, SensorData previous, Func<SensorData, SensorData, double> getValueDiff)
        {
            var timeDiff = current.TimeStamp - previous.TimeStamp;
            var valueDiff = Math.Abs(getValueDiff(current, previous));
            return timeDiff.TotalMilliseconds == 0 ? 0 : (SPEED_TIME_MS * valueDiff) / Math.Abs(timeDiff.TotalMilliseconds);
        }
    }
}